#ifndef MOTION_CONTROLLER_H
#define MOTION_CONTROLLER_H

#include <Arduino.h>
#include <AccelStepper.h>
#include <MultiStepper.h>

// 前向声明，避免循环依赖
class LEDManager;
class LEDControlCenter;

// 电机状态枚举（从stepper_controller移植）
enum MotorStatus {
  MOTOR_IDLE = 0,
  MOTOR_MOVING,
  MOTOR_HOMING,
  MOTOR_ERROR
};

// 电机参数配置 - 统一管理所有电机参数
extern int ROTATION_STEPS_PER_REV;  // 旋转轴每圈步数 (默认8200)
extern int RADIUS_MAX_STEPS;        // 半径轴最大步数 (默认4400)
extern float COMPENSATION_FACTOR;   // 补偿系数 (默认0.25)
extern int MAX_SPEED_SPS;          // 最大速度(步/秒) (默认400)

// 归零相关参数（从stepper_controller移植）
extern int HOMING_SPEED_SPS;       // 归零速度(步/秒) (默认2000)
extern int MAX_HOMING_STEPS_THETA; // 角度轴最大归零步数 (默认14746)  
extern int MAX_HOMING_STEPS_RHO;   // 半径轴最大归零步数 (默认8960)

// 🔧 新增：结构参数 - 用于动态计算标定值
extern int BASE_ROT_STEPS;         // 基础角度轴步数（如28BYJ-48的标准步数）
extern int BASE_RAD_STEPS;         // 基础半径轴步数
extern float ROT_STEPS_MULTIPLIER; // 角度轴乘数因子（rotsteps * 3）
extern float RAD_STEPS_MULTIPLIER; // 半径轴乘数因子（radsteps * 1.1）

// 运动状态结构
struct Position {
    float theta;    // 弧度
    float rho;      // 0-1范围
    long rotation_steps;    // 旋转轴步数
    long radius_steps;      // 半径轴步数
};

class MotionController {
private:
    AccelStepper* rotationMotor;
    AccelStepper* radiusMotor;
    MultiStepper steppers;
    
    Position currentPosition;
    Position targetPosition;
    
    bool isMoving;
    unsigned long lastMoveTime;
    
    // 扫图控制（独立于isMoving的扫图状态控制）
    bool sweepPaused;
    
    // 精确补偿累积器
    float accumulatedCompensation;
    
    // LED管理器支持（按照附件验证方案）
    LEDManager* ledManager;
    LEDControlCenter* ledControlCenter;

    // 归零相关状态（从stepper_controller移植）
    bool isHoming;
    MotorStatus thetaStatus;
    MotorStatus rhoStatus;
    int thetaHomePin, rhoHomePin;
    
    // 电机自动管理（从stepper_controller移植）
    unsigned long lastMotorActivity;
    unsigned long motorIdleTimeout;
    bool autoMotorManagement;
    bool motorsEnabled;
    
    // 标定参数（从stepper_controller移植）
    float thetaStepsPerDegree;
    float rhoStepsPerMM;
    
    // 半径轴补偿（从stepper_controller移植）
    bool radiusCompensationEnabled;
    float radiusCompensationRatio;

public:
    // 构造函数 - 添加归零引脚参数
    MotionController(AccelStepper* rotMotor, AccelStepper* radMotor, int thetaHome = -1, int rhoHome = -1);
    void init();
    
    // 运动控制（保持原有绘画功能不变）
    void moveTo(float theta, float rho);
    void moveToSteps(long rotationSteps, long radiusSteps);
    bool update(); // 非阻塞运动更新，返回是否完成
    void stop();   // 暂停运动
    void start();  // 继续运动
    
    // 坐标转换
    void polarToSteps(float theta, float rho, long& rotationSteps, long& radiusSteps);
    void stepsToPolar(long rotationSteps, long radiusSteps, float& theta, float& rho);
    
    // 补偿计算
    long calculateRadiusCompensation(long rotationStepsDelta);
    void resetCompensation();  // 重置累积补偿
    
    // 状态查询
    Position getCurrentPosition();
    bool isMovingNow();
    
    // === 新增：归零功能（从stepper_controller整合） ===
    bool homeMotorsWithCompensation(); // 智能同步归零
    bool isHomed();                    // 检查是否已归零
    bool isHomingInProgress();         // 检查是否正在归零
    
    // === 新增：扫图功能 ===
    bool executeSweepPattern();       // 执行扫图整理沙盘
    bool executeSweepAndHome();       // 完整扫图流程：归零→扫图→归零
    
    // === 新增：电机管理功能（从stepper_controller整合） ===
    void enableMotors();
    void disableMotors();
    bool areMotorsEnabled();
    
    // 电机自动管理
    void enableAutoMotorManagement(bool enable = true);
    void setMotorIdleTimeout(unsigned long timeoutMs);
    void updateMotorActivity();
    void checkMotorIdle();
    bool isAutoMotorManagementEnabled() const;
    unsigned long getMotorIdleTimeout() const;
    unsigned long getLastMotorActivity() const;
    
    // === 新增：状态查询（从stepper_controller整合） ===
    MotorStatus getThetaStatus();
    MotorStatus getRhoStatus();
    void printMotorStatus();
    
    // === 新增：标定功能（从stepper_controller整合） ===
    void setThetaStepsPerDegree(float steps);
    void setRhoStepsPerMM(float steps); 
    float getThetaStepsPerDegree() const;
    float getRhoStepsPerMM() const;
    void printCalibrationInfo();
    void resetCalibration();
    void updateCalibrationFromStructure(); // 🔧 新增：根据结构参数更新标定值
    void rotateByDegrees(float degrees);  // 标定用角度转动
    void moveByDistance(float mm);        // 标定用距离移动
    
    // === 新增：半径轴补偿功能（从stepper_controller整合） ===
    void enableRadiusCompensation(bool enable = true);
    void setRadiusCompensationRatio(float ratio);
    void calculateRadiusCompensationForHoming(long thetaDelta, long& rhoCompensation);
    
    // 配置方法（统一电机参数管理）
    void setCompensationFactor(float factor);
    void setMaxSpeed(float stepsPerSecond);
    void setAcceleration(float stepsPerSecondPerSecond);
    void setHomingSpeed(float stepsPerSecond);  // 新增：归零速度设置
    
    // 配置参数设置 - 🔧 修改为完整函数实现，支持标定更新
    void setRotationStepsPerRev(int steps);
    void setRadiusMaxSteps(int steps);
    
    // 获取配置参数 - 成员函数版本
    int getRotationStepsPerRev() { return ROTATION_STEPS_PER_REV; }
    int getRadiusMaxSteps() { return RADIUS_MAX_STEPS; }
    
    // 🔧 静态参数访问函数 - 供StepperController使用
    static float getCompensationFactorStatic() { return COMPENSATION_FACTOR; }
    static int getMaxSpeedStatic() { return MAX_SPEED_SPS; }
    static int getHomingSpeedStatic() { return HOMING_SPEED_SPS; }
    static float getDrawingSpeedFactor() { return 1.0f; } // 绘图速度系数，默认1.0
    
    // 成员函数版本 - 供内部使用
    float getCompensationFactor() { return COMPENSATION_FACTOR; }
    int getMaxSpeed() { return MAX_SPEED_SPS; }
    int getHomingSpeed() { return HOMING_SPEED_SPS; }
    
    // === 新增：基本移动功能（从StepperController整合） ===
    void moveTheta(int steps);
    void moveRho(int steps);
    void run();  // 电机运行更新
    long getThetaPosition();
    long getRhoPosition();
    void getCurrentCoords(float& theta, float& rho);
    void forceSetPosition(long thetaPos, long rhoPos);
    void resetPosition();
    void printCurrentPosition();
    bool selfTest();
    
    // 向后兼容的参数设置（弃用提示）
    void setDrawingSpeed(float speed);
    void setStepsPerRevolution(int steps);
    void setMaxRadius(float radius);
    
    // 调试和状态信息
    void printStatus();
    void printPosition();
    void resetToZero();
    void forceResetFromHoming();  // 从归零操作后强制同步重置
    
    // 位置同步
    void syncPosition();  // 从电机同步当前位置

    // LED位置更新支持（按照附件验证方案）
    void setLEDManager(LEDManager* manager);
    void setLEDControlCenter(LEDControlCenter* center);

private:
    // 辅助函数（从stepper_controller移植）
    const char* getMotorStatusString(MotorStatus status);
    void configure28BYJ48(); // 28BYJ-48电机配置
    
    // 新增的优化辅助函数
    void runMotorsUntilComplete();
    void runSingleMotorUntilComplete(AccelStepper* motor, const char* motorName);
    void configureMotorSpeed(AccelStepper* motor, int speed);
};

#endif
